
local abs = math.abs
local floor = math.floor
local globalRotationSpeed = 0
local leftRedstone = 0
local rightRedstone = 0

local ec = peripheral.wrap("EngineController_0")
local function getSpeed()
    local sqrt = math.sqrt
    local finalSpeed = sqrt(ec.getVelocity().x^2 + ec.getVelocity().y^2 + ec.getVelocity().z^2)
    finalSpeed = floor(finalSpeed * (18/5))
    return finalSpeed
end

local leftWheels = peripheral.wrap("Create_RotationSpeedController_0")
local rightWheels = peripheral.wrap("Create_RotationSpeedController_1")
leftWheels.setTargetSpeed(0)
rightWheels.setTargetSpeed(0)

local start = os.epoch("utc")
local function yield()
    if (os.epoch("utc") - start > 1000) then
        os.queueEvent("yield")
        os.pullEvent("yield")
        start = os.epoch("utc")
    end
end

local ccRd = peripheral.wrap("top")
ccRd.clearHandles()
sleep(1)
local handles = {}
local links = {}
local links_config = {
    link_W = {"minecraft:red_wool","minecraft:lever"},
    link_S = {"minecraft:blue_wool","minecraft:lever"},
    link_A = {"minecraft:lime_wool","minecraft:lever"},
    link_D = {"minecraft:yellow_wool","minecraft:lever"},
    link_Space = {"minecraft:lever","minecraft:lever"},
    link_Engine = {"minecraft:redstone","minecraft:redstone"}
}

--load the handles (if there is existing handle)
for temNothing,linkFrequency in pairs(ccRd.getHandles()) do
    local items = linkFrequency.getItems()
    handles[ items[1]..items[2] ] = linkFrequency
end

--Assign relevant handles
for id,items in pairs(links_config) do
    links[id] = handles[ items[1]..items[2] ]
            or ccRd.openHandle(items[1],items[2])
end

if ccRd then print(114514) end


--wheels redstones:
-- 0 = left; 1 = right
local rd0 = peripheral.wrap("tm_rsPort_0")
local rd1 = peripheral.wrap("tm_rsPort_1")

local rd2 = peripheral.wrap("tm_rsPort_2")
local rd3 = peripheral.wrap("tm_rsPort_3")

local rd4 = peripheral.wrap("tm_rsPort_4")
local rd5 = peripheral.wrap("tm_rsPort_5")

local rd6 = peripheral.wrap("tm_rsPort_6")
local rd7 = peripheral.wrap("tm_rsPort_7")

local turningLeftN = 1
local turningRightN = 1

local function isPressW()
    local boo = false
    if links.link_W.getSignal() ~= 0 then
        --print("hey")
        boo = true
    end
    return boo
end

local function isPressS()
    local boo = false
    if links.link_S.getSignal() ~= 0 then
        boo = true
    end
    return boo
end

local function isPressA()
    local boo = false
    if links.link_A.getSignal() ~= 0 then
        boo = true
    end
    return boo
end

local function isPressD()
    local boo = false
    if links.link_D.getSignal() ~= 0 then
        boo = true
    end
    return boo
end

local function isPressSpace()
    local boo = false
    if links.link_Space.getSignal() ~= 0 then
        boo = true
    end
    return boo
end

local function stopTurning()
    local leftRD = leftRedstone
    local rightRD = rightRedstone



    if rightRD == 0 and leftRD ~= 0 then

        local speed = getSpeed()
        if speed >= 36 then
            turningLeftN = 0.95
            turningRightN = 1
        else
            turningLeftN = 0.75
            turningRightN = 1

        end

        if leftRedstone <= 4 and leftRedstone > 0 then
            leftRedstone = leftRedstone - 1
            rd0.setAnalogOutput("down",leftRedstone)
            rd2.setAnalogOutput("down",0)
            rd5.setAnalogOutput("down",0)
            rd7.setAnalogOutput("down",0)
        elseif leftRedstone > 4 and leftRedstone <= 6 then
            leftRedstone = leftRedstone - 1
            rd0.setAnalogOutput("down",leftRedstone)
            rd2.setAnalogOutput("down",leftRedstone - 4)
            rd5.setAnalogOutput("down",0)
            rd7.setAnalogOutput("down",0)
        elseif leftRedstone > 6 and leftRedstone <= 8 then
            leftRedstone = leftRedstone - 1
            rd0.setAnalogOutput("down",leftRedstone)
            rd2.setAnalogOutput("down",leftRedstone - 4)
            rd5.setAnalogOutput("down",0)
            rd7.setAnalogOutput("down",leftRedstone - 6)
        elseif leftRedstone > 8 then
            leftRedstone = leftRedstone - 1
            rd0.setAnalogOutput("down",leftRedstone)
            rd2.setAnalogOutput("down",leftRedstone - 4)
            rd5.setAnalogOutput("down",leftRedstone - 8)
            rd7.setAnalogOutput("down",leftRedstone - 6)
        end
    elseif leftRD == 0 and rightRD ~= 0 then
        local speed = getSpeed()
        if speed >= 36 then
            turningLeftN = 1
            turningRightN = 0.95
        else
            turningLeftN = 1
            turningRightN = 0.75

        end

        if rightRedstone <= 4 then
            rightRedstone = rightRedstone - 1
            rd1.setAnalogOutput("down",rightRedstone)
            rd3.setAnalogOutput("down",0)
            rd4.setAnalogOutput("down",0)
            rd6.setAnalogOutput("down",0)
        elseif rightRedstone > 4 and rightRedstone <= 6 then
            rightRedstone = rightRedstone - 1
            rd1.setAnalogOutput("down",rightRedstone)
            rd3.setAnalogOutput("down",rightRedstone - 4)
            rd4.setAnalogOutput("down",0)
            rd6.setAnalogOutput("down",0)
        elseif rightRedstone > 6 and rightRedstone <= 8 then
            rightRedstone = rightRedstone - 1
            rd1.setAnalogOutput("down",rightRedstone)
            rd3.setAnalogOutput("down",rightRedstone - 4)
            rd4.setAnalogOutput("down",0)
            rd6.setAnalogOutput("down",rightRedstone - 6)
        elseif rightRedstone > 8 then
            rightRedstone = rightRedstone - 1
            rd1.setAnalogOutput("down",rightRedstone)
            rd3.setAnalogOutput("down",rightRedstone - 4)
            rd4.setAnalogOutput("down",rightRedstone - 8)
            rd6.setAnalogOutput("down",rightRedstone - 6)
        end
    elseif rightRD == 0 and leftRD == 0 then
        turningLeftN = 1
        turningRightN = 1
        rd0.setOutput("down",false)
        rd2.setOutput("down",false)
        rd5.setOutput("down",false)
        rd7.setOutput("down",false)
        rd1.setOutput("down",false)
        rd3.setOutput("down",false)
        rd4.setOutput("down",false)
        rd6.setOutput("down",false)

    end
end

local function pause(boo)
    local isForcePause = boo
    local pauseForce = 0
    if isForcePause then
        pauseForce = 10
    else
        pauseForce = 1
    end

    local lSpeed = leftWheels.getTargetSpeed()
    local rSpeed = rightWheels.getTargetSpeed()

    if lSpeed < 0 and rSpeed < 0 then
            if globalRotationSpeed < -pauseForce then
                globalRotationSpeed = globalRotationSpeed + pauseForce
            else
                globalRotationSpeed = 0
            end

    elseif lSpeed > 0 and rSpeed > 0 then
            if globalRotationSpeed > pauseForce then
                globalRotationSpeed = globalRotationSpeed - pauseForce
            else
                globalRotationSpeed = 0
            end
    end
    leftWheels.setTargetSpeed(floor(globalRotationSpeed * turningLeftN))
    rightWheels.setTargetSpeed(floor(globalRotationSpeed * turningRightN))
end

local function handle_W_Press()
    local lSpeed = leftWheels.getTargetSpeed()
    local rSpeed = rightWheels.getTargetSpeed()

    if lSpeed <= 0 and rSpeed <= 0 then
            if globalRotationSpeed > -255 then
                globalRotationSpeed = globalRotationSpeed - 1
            else
                globalRotationSpeed = -256
            end
        leftWheels.setTargetSpeed(floor(globalRotationSpeed * turningLeftN))
        rightWheels.setTargetSpeed(floor(globalRotationSpeed * turningRightN))

    elseif lSpeed > 0 and rSpeed > 0 then
            pause(true)
    end

end

local function handle_S_Press()
    local lSpeed = leftWheels.getTargetSpeed()
    local rSpeed = rightWheels.getTargetSpeed()


    if lSpeed >= 0 and rSpeed >= 0 then

            if globalRotationSpeed < 48 then
                globalRotationSpeed = globalRotationSpeed + 1
            else
                globalRotationSpeed = 48
            end
        leftWheels.setTargetSpeed(floor(globalRotationSpeed * turningLeftN))
        rightWheels.setTargetSpeed(floor(globalRotationSpeed * turningRightN))

    elseif lSpeed < 0 and rSpeed < 0 then
        pause(true)
    end
end

local function handle_A_Press()

    local leftRD = leftRedstone
    local rightRD = rightRedstone

    if rightRD ~= 0 then
        leftRD = 0
        stopTurning()
    else
        local speed = getSpeed()
        if speed >= 36 then
            turningLeftN = 0.95
            turningRightN = 1
        else
            turningLeftN = 0.75
            turningRightN = 1

        end

        if leftRedstone < 4 then
            leftRedstone = leftRedstone + 1
            rd0.setAnalogOutput("down",leftRedstone)
            rd2.setAnalogOutput("down",0)
            rd5.setAnalogOutput("down",0)
            rd7.setAnalogOutput("down",0)
        elseif leftRedstone >=4 and leftRedstone < 6 then
            leftRedstone = leftRedstone + 1
            rd0.setAnalogOutput("down",leftRedstone)
            rd2.setAnalogOutput("down",leftRedstone - 4)
            rd5.setAnalogOutput("down",0)
            rd7.setAnalogOutput("down",0)
        elseif leftRedstone >= 6 and leftRedstone < 8 then
            leftRedstone = leftRedstone + 1
            rd0.setAnalogOutput("down",leftRedstone)
            rd2.setAnalogOutput("down",leftRedstone - 4)
            rd5.setAnalogOutput("down",0)
            rd7.setAnalogOutput("down",leftRedstone - 6)
        elseif leftRedstone >= 8 and leftRedstone < 15 then
            leftRedstone = leftRedstone + 1
            rd0.setAnalogOutput("down",leftRedstone)
            rd2.setAnalogOutput("down",leftRedstone - 4)
            rd5.setAnalogOutput("down",leftRedstone - 8)
            rd7.setAnalogOutput("down",leftRedstone - 6)
        elseif leftRedstone == 15 then
            leftRedstone = 15
            rd0.setAnalogOutput("down",leftRedstone)
            rd2.setAnalogOutput("down",leftRedstone - 4)
            rd5.setAnalogOutput("down",leftRedstone - 8)
            rd7.setAnalogOutput("down",leftRedstone - 6)
        end
    end

end


local function handle_D_Press()
    local leftRD = leftRedstone
    local rightRD = rightRedstone

    if leftRD ~= 0 then
        rightRD = 0
        stopTurning()
    else
        local speed = getSpeed()
        if speed >= 36 then
            turningLeftN = 1
            turningRightN = 0.95
        else
            turningLeftN = 1
            turningRightN = 0.75

        end

        if rightRedstone < 4 then
            rightRedstone = rightRedstone + 1
            rd1.setAnalogOutput("down",rightRedstone)
            rd3.setAnalogOutput("down",0)
            rd4.setAnalogOutput("down",0)
            rd6.setAnalogOutput("down",0)
        elseif rightRedstone >=4 and rightRedstone < 6 then
            rightRedstone = rightRedstone + 1
            rd1.setAnalogOutput("down",rightRedstone)
            rd3.setAnalogOutput("down",rightRedstone - 4)
            rd4.setAnalogOutput("down",0)
            rd6.setAnalogOutput("down",0)
        elseif rightRedstone >= 6 and rightRedstone < 8 then
            rightRedstone = rightRedstone + 1
            rd1.setAnalogOutput("down",rightRedstone)
            rd3.setAnalogOutput("down",rightRedstone - 4)
            rd4.setAnalogOutput("down",0)
            rd6.setAnalogOutput("down",rightRedstone - 6)
        elseif rightRedstone >= 8 and rightRedstone < 15 then
            rightRedstone = rightRedstone + 1
            rd1.setAnalogOutput("down",rightRedstone)
            rd3.setAnalogOutput("down",rightRedstone - 4)
            rd4.setAnalogOutput("down",rightRedstone - 8)
            rd6.setAnalogOutput("down",rightRedstone - 6)
        elseif rightRedstone == 15 then
            rightRedstone = 15
            rd1.setAnalogOutput("down",rightRedstone)
            rd3.setAnalogOutput("down",rightRedstone - 4)
            rd4.setAnalogOutput("down",rightRedstone - 8)
            rd6.setAnalogOutput("down",rightRedstone - 6)
        end
    end

end


local function checkIfRdInput()
    local boo = false
    local gotoReturn = false

    if isPressSpace() then
        boo = true
        if isPressA() then
            handle_A_Press()
        elseif isPressD() then
            handle_D_Press()
        else
            stopTurning()
        end
        pause(boo)
        gotoReturn = true
    end

    if isPressW() and not gotoReturn then
        boo = true
        if isPressA() then
            handle_A_Press()
        elseif isPressD() then
            handle_D_Press()
        else
            stopTurning()
        end
        handle_W_Press()
        gotoReturn = true

    elseif isPressS() and not gotoReturn then
        boo = true
        if isPressA() then
            handle_A_Press()
        elseif isPressD() then
            handle_D_Press()
        else
            stopTurning()
        end
        handle_S_Press()
        gotoReturn = true
    end


    if isPressD() and not gotoReturn then
        boo = true
        handle_D_Press()
        pause(false)
        gotoReturn = true
    end

    if isPressA() and not gotoReturn then
        boo = true
        handle_A_Press()
        pause(false)
        gotoReturn = true
    end

    return boo

end

shell.openTab("aScreen.lua &")
while true do
    local boo = checkIfRdInput()
    if not boo then
        stopTurning()
        pause(false)
    end
    yield()

end